/**
 * @file test_hrt_call.c
 * 
 * Unit tests for HRT (High Resolution Timer) callout system
 */

#include "px4_platform_common/hrt_call.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stdio.h>
#include <stdbool.h>

/* External debug printf function */
extern int debug_printf(const char *format, ...);

/* Test statistics */
static int tests_passed = 0;
static int tests_failed = 0;

/* Test callback counters */
static volatile int callback_count = 0;
static volatile int callback_arg_value = 0;
static volatile hrt_abstime callback_time = 0;

/* Test macros */
#define TEST_ASSERT(condition, msg) \
    do { \
        if (condition) { \
            tests_passed++; \
            debug_printf("[PASS] %s\r\n", msg); \
        } else { \
            tests_failed++; \
            debug_printf("[FAIL] %s\r\n", msg); \
        } \
    } while(0)

/**
 * @brief Test callback function
 */
static void test_callback(void *arg)
{
    callback_count++;
    callback_arg_value = (int)(uintptr_t)arg;
    callback_time = hrt_absolute_time();
}

/**
 * @brief Test 1: HRT absolute time
 */
static void test_hrt_absolute_time(void)
{
    debug_printf("\r\n=== Test: HRT Absolute Time ===\r\n");
    
    /* Get initial time */
    hrt_abstime t1 = hrt_absolute_time();
    TEST_ASSERT(t1 > 0, "hrt_absolute_time returns non-zero");
    
    /* Wait 10ms */
    vTaskDelay(pdMS_TO_TICKS(10));
    
    /* Get time again */
    hrt_abstime t2 = hrt_absolute_time();
    TEST_ASSERT(t2 > t1, "Time increases monotonically");
    
    /* Check that elapsed time is approximately 10ms (allow ±2ms tolerance) */
    hrt_abstime elapsed = t2 - t1;
    TEST_ASSERT(elapsed >= 8000 && elapsed <= 12000, "Elapsed time ~10ms");

    debug_printf("  Elapsed: %lu us (expected ~10000 us)\r\n", (uint32_t)elapsed);
}

/**
 * @brief Test 2: One-shot timer (hrt_call_after)
 */
static void test_hrt_call_after(void)
{
    debug_printf("\r\n=== Test: One-shot Timer (hrt_call_after) ===\r\n");
    
    /* Initialize callout */
    hrt_call_t call;
    hrt_call_init(&call);
    TEST_ASSERT(call.timer != NULL, "hrt_call_init creates timer");
    
    /* Reset callback counter */
    callback_count = 0;
    callback_arg_value = 0;
    
    /* Schedule callback after 20ms */
    hrt_abstime start_time = hrt_absolute_time();
    hrt_call_after(&call, 20000, test_callback, (void *)42);
    TEST_ASSERT(call.active, "Timer is active after scheduling");
    
    /* Wait for callback */
    vTaskDelay(pdMS_TO_TICKS(30));
    
    /* Check callback was called */
    TEST_ASSERT(callback_count == 1, "Callback called once");
    TEST_ASSERT(callback_arg_value == 42, "Callback received correct argument");
    TEST_ASSERT(!call.active, "Timer is inactive after one-shot");
    
    /* Check timing (allow ±5ms tolerance) */
    hrt_abstime elapsed = callback_time - start_time;
    TEST_ASSERT(elapsed >= 15000 && elapsed <= 25000, "Callback timing ~20ms");

    debug_printf("  Callback time: %lu us (expected ~20000 us)\r\n", (uint32_t)elapsed);

    /* Cleanup */
    hrt_cancel(&call);
}

/**
 * @brief Test 3: Absolute time timer (hrt_call_at)
 */
static void test_hrt_call_at(void)
{
    debug_printf("\r\n=== Test: Absolute Time Timer (hrt_call_at) ===\r\n");
    
    /* Initialize callout */
    hrt_call_t call;
    hrt_call_init(&call);
    
    /* Reset callback counter */
    callback_count = 0;
    
    /* Schedule callback at absolute time (now + 15ms) */
    hrt_abstime start_time = hrt_absolute_time();
    hrt_abstime target_time = start_time + 15000;
    hrt_call_at(&call, target_time, test_callback, (void *)99);
    
    /* Wait for callback */
    vTaskDelay(pdMS_TO_TICKS(25));
    
    /* Check callback was called */
    TEST_ASSERT(callback_count == 1, "Callback called once");
    TEST_ASSERT(callback_arg_value == 99, "Callback received correct argument");
    
    /* Check timing */
    hrt_abstime elapsed = callback_time - start_time;
    TEST_ASSERT(elapsed >= 10000 && elapsed <= 20000, "Callback timing ~15ms");

    debug_printf("  Callback time: %lu us (expected ~15000 us)\r\n", (uint32_t)elapsed);

    /* Cleanup */
    hrt_cancel(&call);
}

/**
 * @brief Test 4: Periodic timer (hrt_call_every)
 */
static void test_hrt_call_every(void)
{
    debug_printf("\r\n=== Test: Periodic Timer (hrt_call_every) ===\r\n");

    /* Initialize callout */
    hrt_call_t call;
    hrt_call_init(&call);

    /* Reset callback counter */
    callback_count = 0;

    /* Schedule periodic callback (10ms initial delay, 10ms period) */
    hrt_call_every(&call, 10000, 10000, test_callback, (void *)123);
    TEST_ASSERT(call.active, "Timer is active after scheduling");
    TEST_ASSERT(call.period == 10000, "Period is set correctly");

    /* Wait for multiple callbacks (50ms = ~4-5 callbacks) */
    vTaskDelay(pdMS_TO_TICKS(50));

    /* Check callback was called multiple times */
    TEST_ASSERT(callback_count >= 3 && callback_count <= 6, "Callback called 3-6 times");
    TEST_ASSERT(callback_arg_value == 123, "Callback received correct argument");
    TEST_ASSERT(call.active, "Timer still active (periodic)");
    TEST_ASSERT(!hrt_called(&call), "hrt_called returns false for periodic timer");

    debug_printf("  Callback count: %d (expected ~4-5)\r\n", callback_count);

    /* Cancel timer */
    hrt_cancel(&call);
    TEST_ASSERT(!call.active, "Timer is inactive after cancel");

    /* Wait and verify no more callbacks */
    int count_before_cancel = callback_count;
    vTaskDelay(pdMS_TO_TICKS(30));
    TEST_ASSERT(callback_count == count_before_cancel, "No callbacks after cancel");
}

/**
 * @brief Test 5: Timer cancellation
 */
static void test_hrt_cancel(void)
{
    debug_printf("\r\n=== Test: Timer Cancellation ===\r\n");

    /* Initialize callout */
    hrt_call_t call;
    hrt_call_init(&call);

    /* Reset callback counter */
    callback_count = 0;

    /* Schedule callback after 30ms */
    hrt_call_after(&call, 30000, test_callback, (void *)0);
    TEST_ASSERT(call.active, "Timer is active after scheduling");

    /* Cancel immediately */
    hrt_cancel(&call);
    TEST_ASSERT(!call.active, "Timer is inactive after cancel");
    TEST_ASSERT(call.deadline == 0, "Deadline cleared after cancel");

    /* Wait and verify callback was not called */
    vTaskDelay(pdMS_TO_TICKS(40));
    TEST_ASSERT(callback_count == 0, "Callback not called after cancel");
    TEST_ASSERT(hrt_called(&call), "hrt_called returns true after cancel");
}

/**
 * @brief Test 6: Timer rescheduling
 */
static void test_hrt_reschedule(void)
{
    debug_printf("\r\n=== Test: Timer Rescheduling ===\r\n");

    /* Initialize callout */
    hrt_call_t call;
    hrt_call_init(&call);

    /* Reset callback counter */
    callback_count = 0;

    /* Schedule callback after 50ms */
    hrt_call_after(&call, 50000, test_callback, (void *)1);

    /* Wait 10ms and reschedule to 20ms */
    vTaskDelay(pdMS_TO_TICKS(10));
    hrt_call_after(&call, 20000, test_callback, (void *)2);

    /* Wait for callback */
    vTaskDelay(pdMS_TO_TICKS(30));

    /* Check callback was called once with new argument */
    TEST_ASSERT(callback_count == 1, "Callback called once after reschedule");
    TEST_ASSERT(callback_arg_value == 2, "Callback received new argument");

    /* Cleanup */
    hrt_cancel(&call);
}

/**
 * @brief Run all HRT callout tests
 */
void hrt_call_run_tests(void)
{
    debug_printf("\r\n========================================\r\n");
    debug_printf("  PX4 HRT Callout Unit Tests\r\n");
    debug_printf("========================================\r\n");

    tests_passed = 0;
    tests_failed = 0;

    /* Run tests */
    test_hrt_absolute_time();
    test_hrt_call_after();
    test_hrt_call_at();
    test_hrt_call_every();
    test_hrt_cancel();
    test_hrt_reschedule();

    /* Print results */
    debug_printf("\r\n========================================\r\n");
    debug_printf("  Test Results\r\n");
    debug_printf("========================================\r\n");
    debug_printf("  Passed: %d\r\n", tests_passed);
    debug_printf("  Failed: %d\r\n", tests_failed);
    debug_printf("  Total:  %d\r\n", tests_passed + tests_failed);
    debug_printf("========================================\r\n\r\n");
}

